#ifndef _VISGRABTYPES_H_
#define _VISGRABTYPES_H_

#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <algorithm>
#include <iostream>
#include <fstream>

#include <opencv/cv.h>
#include <opencv/highgui.h>

#define NR_JOINTS_SDH 7   //!< Number of joints of the Schunk Dexterous Hand (=7)


//! Class containing contact-point information.
/*!
 * Contact point information: 3D pose, \f$\mathbf{x}_i\f$, and 3D unit vector, \f$\overrightarrow{\mathbf{d}_i}\f$, 
 * giving the preferred force direction of the finger (see figure). 
 * \image html pictures/grasp_representation_contact.png
 */
class GraspContactPoint {
 public:
  //! Constructor function, sets the (x,y,z) position and (d1,d2,d3) direction vector. The direction vector is normalized to unit length 
 GraspContactPoint(double _x, double _y, double _z, double _d1, double _d2, double _d3) :
  x(_x), y(_y), z(_z), d1(_d1), d2(_d2), d3(_d3) {
   double ld = std::sqrt(d1*d1+d2*d2+d3*d3);
    if(ld>0) { d1 /= ld; d2 /= ld; d3 /= ld; }
    else { d1 = 1.00; d2 = 0.0; d3 = 0.0; };
  };    
  //! Function to print the contact-point information 
  void print() {
    std::cout << "      CP, pos: (" << x << ", "  << y << ", "  << z << "), dir: (" 
	      << d1 << ", "  << d2 << ", "  << d3 << ")" << std::endl; 
  };
  double x;  //!< x position of the contact points in mm
  double y;  //!< y position of the contact points in mm
  double z;  //!< z position of the contact points in mm
  double d1; //!< 1st element of the direction vector
  double d2; //!< 2nd element of the direction vector
  double d3; //!< 3rd element of the direction vector
};

//! Class containing information about a grasp in contact-point representation.
/*!
 * Contact-point grasp representation. The grasp is represented by two or three contact point and a preferred
 * approach direction of the hand, \f$\overrightarrow{\mathbf{a}}\f$, which is a 3D vector of unit length.
 * See the figure for an illustration of the representation. 
 *
 * Based on this representation, the grasp simulator tries to find a pose and joint configuration of the hand using
 * a gradient-based inverse-kinematic (IK) solver. 
 * 
 * Note that it is often not possible to exactly match the position and 
 * direction of the contact points. The IK solver minimizes the distance.
 *
 * The IK optimization starts with the hand 
 * orientation set to the preferred approach direction and solutions are only accepted where the orientation of the hand does
 * not differ from the preferred approach direction by more than 90 degrees. 
 * 
 * \image html pictures/grasp_representation_contact.png
 * 3D illustration of the contact-point grasp representation. \f$\mathbf{x}_i\f$ is the set of contact
 * positions, \f$\overrightarrow{\mathbf{d}_i}\f$ the set of contact directions, and \f$\overrightarrow{\mathbf{a}}\f$
 * the approach vector
 *
 */
class GraspRepresentationI {
public:
  //! Constructor function, creates an array to stor the approach vector.
  GraspRepresentationI() {
    approachDirection = new double[3];
    aOpen = new double[7];
    aClose = new double[7];
  };
  //! Desctructor function, deletes all contact points and the array with the direction vector. 
  ~GraspRepresentationI() {
    for(unsigned int i=0; i<contactPoints.size(); i++)
      delete contactPoints[i];
    delete[] approachDirection;
    delete[] aOpen;
    delete[] aClose;
  };
  //! Function to add a contact point. A pointer to a new GraspContactPoint is created and added to the contactPoints vector.
  void addContactPoint(double x, double y, double z, double d1, double d2, double d3) {
    GraspContactPoint* cp = new GraspContactPoint(x, y, z, d1, d2, d3);
    contactPoints.push_back(cp);
  };
  //! Function to set the approach vector. The vector is normalized to unit length.
  void setApproachDirection(double d1, double d2, double d3) {
    double ld = std::sqrt(d1*d1+d2*d2+d3*d3);
    if(ld>0) { d1 /= ld; d2 /= ld; d3 /= ld; }
    else { d1 = 1.00; d2 = 0.0; d3 = 0.0; };
    approachDirection[0] = d1; approachDirection[1] = d2; approachDirection[2] = d3;
  };
  //! Function to set the array containing the angles to add to the joint configuration for the opening configuration
  void setAOpen(double a) {
    aOpen[0] = a; aOpen[1] = 0.0; aOpen[2] = 0.0;
    aOpen[3] = a; aOpen[4] = 0.0; 
    aOpen[5] = a; aOpen[6] = 0.0;    
  };
  //! Function to set the array containing the angles to add to the joint configuration for the closing configuration
  void setAClose(double a) {
    aClose[0] = a; aClose[1] = 0.0; aClose[2] = 0.0;
    aClose[3] = a; aClose[4] = 0.0; 
    aClose[5] = a; aClose[6] = 0.0;    
  };
  //! Function to print the grasp information
  void print() {
    std::cout << "    " << contactPoints.size() << "-finger grasp" << std::endl;
    for(unsigned int i=0; i<contactPoints.size(); i++)
      contactPoints[i]->print();
    std::cout << "      Approach direction: (" 
	 << approachDirection[0] << ", "  << approachDirection[1] << ", " << approachDirection[2] << ")" << std::endl;
  };
  int overallGraspID;                  //!< Overall ID of the grasp
  std::vector< GraspContactPoint* > contactPoints; //!< Vector containing pointers to GraspContactPoint
  double* approachDirection;           //!< Array containing the approach-direction vector
  double* aOpen;                       //!< Array containing the angles to add to the joint configuration for the opening configuration
  double* aClose;                      //!< Array containing the angles to add to the joint configuration for the closing configuration
};

//! Class containing information about the hand pose as position and quaternion.
class HandPose {
public:
  void print() {
    std::cout << "      Hand pose, pos: (" << x << ", "  << y << ", "  << z << "), quat: ("
	 << q0 << ", " << q1 << ", " << q2 << ", " << q3 << ")" << std::endl;
  };
  double x, y, z;        //!< (x,y,z) position of the hand base in mm
  double q0,q1,q2,q3;    //!< Orientation of the hand basegiven as a quaternion rotation
};

//! Class containing information about a grasp in hand pre-shape representation.
/*!
 * Hand pre-shape grasp representation. The grasp is represented by the pose of the hand
 * and a reference to one of the defined hand pre-shapes. Within the VisGraB benchmark, we
 * include three pre-shapes (see figure). More pre-shapes can be disigned by the user.
 * In that case, the user needs to add pre-shapes to the file preShapes.xml.
 * 
 * \image html pictures/preShapes_small.png
 * The pre-defined grasp pre-shapes. The first column shows the two-finger parallel grip (id=0), 
 * the second column show the three-finger ball grip (id=1), and the last column shows the
 * three-finger cylinder grip (id=2).
 */
class GraspRepresentationII {
public:
  //! Constructor function, creates a pointer to a new HandPose object.
  GraspRepresentationII() {
    handPose = new HandPose;
  };
  //! Destructor function, deletes the handPose pointer.
  ~GraspRepresentationII() {
    delete handPose;
  };
  //! Function to set the hand pose as 3D position (x,y,z) and quaternion (q0,...,q3).
  void setHandPose(double x, double y, double z, double q0, double q1, double q2, double q3) {
    handPose->x = x; handPose->y = y; handPose->z = z;
    handPose->q0 = q0; handPose->q1 = q1; handPose->q2 = q2; handPose->q3 = q3;
  };
  //! Function to set the id of the pre-shape. See figure in the class description for an illustration of the available pre-shapes. 
  void setPreShapeID(unsigned int id) {
    preShapeID = id;
  };
  //! Function to print the pre-shape information
  void print() {
    std::cout << "    Pre-shape: " << preShapeID << std::endl;
    handPose->print();
  };
  HandPose* handPose;         //!< Pose of the hand base
  unsigned int preShapeID;    //!< Pre-shape reference
  int overallGraspID;         //!< Overall ID of the grasp
};

//! Class containing information about a grasp in joint-configuration representation.
/*
 * Joint-configuration representation. The grasp is represented by the pose of the hand
 * base and the joint angles of the 7 joints in the Schunk Dexterous Hand. The figure
 * displays the coordinate frame of the hand base, the dimensions of the hand and the
 * joints.
 *
 * TODO: ADD THE FIGURE ILLUSTRATING THE SDH-2. 
 */
class GraspRepresentationIII {
public:
  //! Constructor function, creates a pointer to a new HandPose object and a array for the joint angles.
  GraspRepresentationIII() {
    jointConfOpen = new double[NR_JOINTS_SDH];
    jointConfClose = new double[NR_JOINTS_SDH];
    maxTau = new double[NR_JOINTS_SDH];
    handPose = new HandPose;
  };
  //! Destructor function, deletes the handPose and joint-angle array.
  ~GraspRepresentationIII() {
    delete[] jointConfOpen;
    delete[] jointConfClose;
    delete[] maxTau;
    delete handPose;
  };
  //! Function to set the hand pose as 3D position (x,y,z) and quaternion (q0,...,q3).
  void setHandPose(double x, double y, double z, double q0, double q1, double q2, double q3) {
    handPose->x = x; handPose->y = y; handPose->z = z;
    handPose->q0 = q0; handPose->q1 = q1; handPose->q2 = q2; handPose->q3 = q3;
  };
  //! Function to set the open joint configuration providing seven individual doubles
  void setJointConfigurationOpen(double j0, double j1, double j2, double j3, double j4, double j5, double j6) {
    jointConfOpen[0] = j0; jointConfOpen[1] = j1; jointConfOpen[2] = j2; jointConfOpen[3] = j3;
    jointConfOpen[4] = j4; jointConfOpen[5] = j5; jointConfOpen[6] = j6;
  };
  //! Function to set the closed joint configuration providing seven individual doubles
  void setJointConfigurationClose(double j0, double j1, double j2, double j3, double j4, double j5, double j6) {
    jointConfClose[0] = j0; jointConfClose[1] = j1; jointConfClose[2] = j2; jointConfClose[3] = j3;
    jointConfClose[4] = j4; jointConfClose[5] = j5; jointConfClose[6] = j6;
  };
  //! Function to set the max torque for all joints providing seven individual doubles
  void setMaxTorques(double t0, double t1, double t2, double t3, double t4, double t5, double t6) {
    maxTau[0] = t0; maxTau[1] = t1; maxTau[2] = t2; maxTau[3] = t3;
    maxTau[4] = t4; maxTau[5] = t5; maxTau[6] = t6;
  };
  //! Function to set the open joint configuration providing an array of doubles that needs to be of length NR_JOINT_SDH (7)
  void setJointConfigurationOpen(double j[]) {
    for(unsigned int i=0; i<NR_JOINTS_SDH; i++)
      jointConfOpen[i] = j[i]; 
  };
  //! Function to set the closed joint configuration providing an array of doubles that needs to be of length NR_JOINT_SDH (7)
  void setJointConfigurationClose(double j[]) {
    for(unsigned int i=0; i<NR_JOINTS_SDH; i++)
      jointConfClose[i] = j[i]; 
  };
  //! Function to set the max torque of all joints providing an array of doubles that needs to be of length NR_JOINT_SDH (7)
  void setMaxTorques(double j[]) {
    for(unsigned int i=0; i<NR_JOINTS_SDH; i++)
      maxTau[i] = j[i]; 
  };
  //! Function to print the grasp representation
  void print() {
    std::cout << "    Hand pose and joint config: " << std::endl;
    handPose->print();
    std::cout << "      Open joint conf: (" << jointConfOpen[0];
    for(int i=1; i<NR_JOINTS_SDH; i++)
      std::cout << ", " << jointConfOpen[i];
    std::cout << ")" << std::endl;
    std::cout << "      Closed joint conf: (" << jointConfClose[0];
    for(int i=1; i<NR_JOINTS_SDH; i++)
      std::cout << ", " << jointConfClose[i];
    std::cout << ")" << std::endl;
    std::cout << "      Max torque for all joints: (" << maxTau[0];
    for(int i=1; i<NR_JOINTS_SDH; i++)
      std::cout << ", " << maxTau[i];
    std::cout << ")" << std::endl;
  };
  HandPose* handPose;      //!< Pose of the hand base
  double* jointConfOpen;   //!< Open joint configuration of the hand
  double* jointConfClose;  //!< Closed joint configuration of the hand
  double* maxTau;          //!< Max torques of all joints of the hand
  int overallGraspID;      //!< Overall ID of the grasp
};

//! Class containing a colored 3D point.
/*
 * The class contains an array for the 3D pose and an array to store 
 * the color color (3D). When an object of the class is deleted, the
 * arrays are deleted as well.
 */
class Point3DCol {
 public:
  //! Constructor function, creates an array for the 3D pose and 3D color information.
  Point3DCol() { pos = new double[3], col = new double[3]; };
  //! Desctructor function, deletes the pose array and the color array.
  ~Point3DCol() { delete[] pos; delete[] col; };
  double *pos;  //!< Array for the (3D) pose
  double *col;  //!< Array for the (3D) color
};

enum PointCloudType {PC_ORIGINAL, PC_SHALLOW_COPY};

//! Class containing a colored point cloud.
/*
 * The class contains a vector of pointers to colored 3D points (Point3DCol).
 * When an object of the class is deleted, all points in the point cloud
 * are deleted as well.
 */
class PointCloud {
public:
  //! Destructor function, deletes all Point3DCol pointers in teh point vector
  PointCloud(PointCloudType _pcType) { pcType = _pcType;};
  ~PointCloud() {
    if(pcType == PC_ORIGINAL) { // Delete the data of all points in the cloud
      for(unsigned int i=0; i<points.size(); i++)
	delete points[i];
    }
  };
  PointCloudType pcType;
  std::vector<Point3DCol*> points;  //!< Vector containing pointers to all points in the cloud
};

//! Class containing a disparity map.
/*
 * The disparity map is contained as a float array. The map is set by passing a pointer to
 * an existing array in the contructor function DisparityMap(float *m, int w, int h). 
 *
 * NB. When an object of this class is deleted, the float array containing the disparity
 * map is deleted if it exists.
 */
class DisparityMap {
public:
  //! Constructor function
  DisparityMap() { map = NULL; };
  //! Constructor function, set the pointer to the map array, the width, and the height.
  DisparityMap(float *m, int w, int h) {
    map = m;
    width = w;
    height = h;
  };
  //! Destructor function, deletes the map array if it exists.
  ~DisparityMap() { if(map) delete[] map;};
  float *map;   //!< Float array to contain the disparity map
  int width;    //!< Width of the map
  int height;   //!< Height of the map
};

//! Class containing information about the VisGraB scene.
class SceneInfo {
public:
  //! Function to print the scene information
  void print() {
    std::cout << "--- sceneID " << sceneID << "---" << std::endl;
    std::cout << "  sceneLocator: " << sceneLocator << std::endl;
    std::cout << "  textureCondition: " << textureCondition << std::endl;
    std::cout << "  objects:";
    for(unsigned int i=0; i<objects.size(); i++)
      std::cout << " " << objects[i];
    std::cout << std::endl;
    std::cout << "  sceneSet: " << sceneSet << std::endl;
    std::cout << "  sceneSetID: " << sceneSetID << std::endl;
    std::cout << "  poseID: " << poseID << std::endl;
    std::cout << "  leftImg: " << leftImg << std::endl;
    std::cout << "  rightImg: " << rightImg << std::endl;
    std::cout << "  disp: " << disp << std::endl;
    std::cout << "  pointCloud: " << pointCloud << std::endl;
    std::cout << "  wc: " << wc << std::endl;
    std::cout << "  dwc: " << dwc << std::endl;
    std::cout << "  collision: " << collision << std::endl;
    std::cout << "------------------------" << std::endl;
  };
  int sceneID;             //!< Scene ID
  std::string sceneLocator;     //!< Scene locator, [texture|non-texture]/<sceneSet>/img_<sceneNr>
  std::string textureCondition; //!< Texture condition, textured or non-textured
  std::string poseCondition;    //!< Scene condition, one objects: standing/laying, two objects:far/close
  std::vector<std::string> objects;  //!< vector of strings with the object(s) in the scene
  int nrObjects;           //!< Number of objects in the scene
  std::string sceneSet;         //!< Name of the scene set (e.g., amicelli, tomato_waterjug, ...)
  int sceneSetID;          //!< Scene-set ID, counting from 0. In order to include in matlab data file
  int poseID;             //!< Scene ID. NB. this is one less than the number behind 'img_', e.g., img_03 -> poseID = 2
  std::string leftImg;          //!< Location of the left rectified image
  std::string rightImg;         //!< Location of the right rectified image
  std::string disp;             //!< Location of the pre-calculated disparity-map file
  std::string pointCloud;       //!< Location of the pre-calculated point-cloud file
  std::string wc;               //!< Location of the RobWork workcell file
  std::string dwc;              //!< Location of the RobWork dynami workcell file
  std::string collision;        //!< Location of the collision file
};

//! Class containing all input data of a given scene
class SceneData {
public:
  /*! \brief
   * Constructor function, reads the left image, right image, disparity map, and point cloud using
   * readLeftRectifiedImage(string fileName), readRightRectifiedImage(string fileName)
   * readDisparityMap(string fileName), and readPointCloud(string fileName).
   */
  SceneData(SceneInfo* sceneInfo, std::string rootPath) { 
    readLeftRectifiedImage(rootPath + "/" + sceneInfo->leftImg);
    readRightRectifiedImage(rootPath + "/" + sceneInfo->rightImg);
    readDisparityMap(rootPath + "/" + sceneInfo->disp);
    readPointCloud(rootPath + "/" + sceneInfo->pointCloud);
  };
  //! Destructor function, deletes the disparity map and the point cloud. The OpenCV Mat objects are smart pointers.
  ~SceneData() { 
    if(disparityMap) delete disparityMap;
    if(pointCloud) delete pointCloud;
  };
  cv::Mat leftRectifiedImage;  //!< OpenCV Mat containing the left rectified image
  cv::Mat rightRectifiedImage; //!< OpenCV Mat containing the right rectified image
  DisparityMap* disparityMap;  //!< Pointer to the disparity map
  PointCloud* pointCloud;      //!< Pointer to the point cloude

 private:
  //! Function to read the left rectified image using the OpenCV function cv::imread
  void readLeftRectifiedImage(std::string fileName) {
    struct stat stInfo;
    if (stat(fileName.c_str(), &stInfo) != 0)  {
      std::cout << "File " + fileName + " does not exist" << std::endl;
      exit(0);
    }
    leftRectifiedImage = cv::imread(fileName);
  };
  //! Function to read the right rectified image using the OpenCV function cv::imread
  void readRightRectifiedImage(std::string fileName) {
    rightRectifiedImage = cv::imread(fileName);
    struct stat stInfo;
    if (stat(fileName.c_str(), &stInfo) != 0)  {
      std::cout << "File " + fileName + " does not exist" << std::endl;
      exit(0);
    }
  };
  //! Function to read the disparity map. Size of the disparity map is 600 x 480.
  void readDisparityMap(std::string fileName) {
    struct stat stInfo;
    if (stat(fileName.c_str(), &stInfo) != 0)  {
      std::cout << "File " + fileName + " does not exist" << std::endl;
      exit(0);
    }
    int width=600; int height=480;
    FILE * fd = fopen(fileName.c_str(), "r");
    float *dispData = new float[width*height];
    size_t numEl = fread(dispData, sizeof(float), width*height, fd);
    fclose(fd);
    if(numEl != (size_t)(width*height)) {
      std::cout << "ERROR: could not read all data of disparity file: " << fileName << std::endl;
      exit(0);
    }
    disparityMap = new DisparityMap(dispData, width, height);
  };
  //! Function to read the point cloud.
  void readPointCloud(std::string fileName) {
    struct stat stInfo;
    if (stat(fileName.c_str(), &stInfo) != 0)  {
      std::cout << "File " + fileName + " does not exist" << std::endl;
      exit(0);
    }
    pointCloud = new PointCloud(PC_ORIGINAL);
    float pt3D[3];
    float col[3];
    std::ifstream file( fileName.c_str() ) ;
    std::string line;  
    while( std::getline( file, line ) ) {
      sscanf(line.c_str(),"%f %f %f %f %f %f\n",&pt3D[0],&pt3D[1],&pt3D[2],&col[0],&col[1],&col[2]);
      Point3DCol *point3D = new Point3DCol;
      point3D->pos[0] = pt3D[0];
      point3D->pos[1] = pt3D[1];
      point3D->pos[2] = pt3D[2];
      point3D->col[0] = col[0];
      point3D->col[1] = col[1];
      point3D->col[2] = col[2];
      pointCloud->points.push_back(point3D);
    }
  };
};

//! Class containing all grasps and the information of a single scene.
/*!
 * When an object of this class is deleted, all elements in the grasps 
 * vector are deleted, as well as the sceneInfo point.
 * The class uses a template, so that it can be used with all three
 * grasp representations.
 */
template <class GraspT>
class SceneGrasps {
public:
  //! Constructor function
  SceneGrasps() { sceneInfo = NULL; };
  //! Destructor function, deletes all grasps associated with the scene and the scene info
  ~SceneGrasps() { 
    for(unsigned int i=0; i<grasps.size(); i++)
      delete grasps[i];
    //for_each(grasps.begin(); grasps.end(); DeleteVectorFuntor<GraspT*>());
    if(sceneInfo)
      delete sceneInfo;
  };
  //! Function to print the scene info and its grasps
  void print() {
    std::cout << "========= scene =========" << std::endl;
    sceneInfo->print();
    std::cout << "========= grasps ========" << std::endl;
    for(unsigned int i=0; i<grasps.size(); i++)
      grasps[i]->print();
    std::cout << "=========================" << std::endl;
    std::cout << std::endl;
  };
  std::vector<GraspT*> grasps;  //!< Vector with pointers to all the grasps in this scene.
  SceneInfo* sceneInfo;    //!< Pointer to scene information
};

struct PoseCondition {
  int nrObjects;
  std::string type;
  int startID, endID;
};

struct PlaneEquation {
  double a,b,c,d;
  double nABC;
  bool estimated;
};

#endif
